﻿
// 2020/1/13: 首个记录
// 2020/3/13: 增加GnssImuLocationMode.IMUOnly/Modified。增加GnssImuSample.SatelliteCount
// 2020/3/24: 增加天线经纬度、海拔信息
// 2020/4/24: 增加arrivalTime（从v6开始Offset可根据Session的UTC开始时间进行同步）
// 2020/7/14: 修正v4解析无位置模式问题
// 2020/11/2: 增加卫星时间类型，卫星时间改为TimWithMS类型表示，新增gnssTimeUTC方法
// 2020/11/11: 支持Spadas7

#ifndef GNSSIMU_SAMPLE_V6_H
#define GNSSIMU_SAMPLE_V6_H

#include "spadas.h"

namespace gnssimu_sample_v6
{
	using namespace spadas;

#if SPADAS_VERSION_MAJOR < 7
	typedef OptionalDouble GeneralElement;
#endif

	enum GnssImuLocationMode
	{
		GILM_NoLocation = 0, // 无位置信息
		GILM_Normal = 1, // 默认模式
		GILM_RTKFixed = 2, // RTK固定解
		GILM_RTKFloat = 3, // RTK浮动解
		GILM_RTD = 4, // RTD
		GILM_IMUOnly = 5, // 仅惯导
		GILM_Modified = 6, // 后期修正
	};

	enum GnssTimeType
	{
		GTT_UTC = 0, // UTC时间
		GTT_GPS = 1, // GPS时间
	};

	struct GnssImuSample
	{
		GlobalTimestamp timeStamp; // 时间戳

		GnssImuLocationMode locationMode; // 位置模式
		OptionalInt satelliteCount; // 卫星数量

		OptionalDouble longitude; // 车前保中心地面经度 deg 东经为正
		OptionalDouble latitude; // 车前保中心地面纬度 deg 北纬为正
		OptionalDouble altitude; // 车前保中心地面海拔 m

		OptionalDouble antennaLongitude; // 天线位置经度 deg 东经为正
		OptionalDouble antennaLatitude; // 天线位置纬度 deg 北纬为正
		OptionalDouble antennaAltitude; // 天线位置海拔 m

		OptionalDouble speed; // 车速 kph
		OptionalDouble orientation; // 朝向 deg CCW为正, 北为0, -180~180

		OptionalDouble pitch; // 俯仰角 deg 车头朝下为正
		OptionalDouble roll; // 横滚角 deg 右侧朝下为正
		OptionalDouble yawRate; // 横摆角速度 deg/s CCW为正
		OptionalDouble ax; // 纵向加速度 m/s2 朝前为正
		OptionalDouble ay; // 横向加速度 m/s2 朝左为正
		OptionalDouble az; // 天向加速度 m/s2 朝上为正

		OptionalDouble slipAngle; // 侧偏角 deg 左转时为正
		OptionalDouble jerkX; // 纵向急动度 m/s3 朝前为正
		OptionalDouble jerkY; // 横向急动度 m/s3 朝左为正

		OptionalDouble arrivalTime; // 数据到达时间戳（相对Session开始时间） 秒
		GnssTimeType gnssTimeType; // 卫星时间类型
		Optional<TimeWithMS> gnssTime; // 卫星时间

		GnssImuSample()
		{
			locationMode = GILM_NoLocation;
			gnssTimeType = GTT_UTC;
		}

		Optional<TimeWithMS> gnssTimeUTC()
		{
			if (gnssTime.isValid())
			{
				if (gnssTimeType == GTT_UTC) return gnssTime;
				else if (gnssTimeType == GTT_GPS) return gnssTime.value() + (-18000);
			}
			return Optional<TimeWithMS>();
		}

		static String getProtocolName(UInt channel)
		{
			if (channel == UINF) return "gnssimu-sample-v6";
			else return (String)"gnssimu-sample-v6@" + channel;
		}

		static String getTitle()
		{
			return "Location mode,Longitude[°],Latitude[°],Altitude[m],Speed[KPH],Orientation[°],Pitch angle[°],Roll angle[°],Yaw rate[°/s],Acceleration-x[m/s2],Acceleration-y[m/s2],Acceleration-z[m/s2],Slip angle[°],Jerk-x[m/s3],Jerk-y[m/s3],Satellite count,Antenna longitude[°],Antenna latitude[°],Antenna altitude[m],GNSS time type,Arrival time[s],GNSS year,GNSS month,GNSS day,GNSS hour,GNSS minute,GNSS second,GNSS millisecond";
		}

		static Bool supportInterpolation()
		{
			return FALSE;
		}

		GeneralSample toGeneralSample()
		{
			GeneralSample sample;
			sample.timeStamp = timeStamp;
			sample.values = Array<GeneralElement>(28);
			GeneralElement *data = sample.values.data();

			data[0] = (Double)(Int)locationMode;
			data[1] = longitude;
			data[2] = latitude;
			data[3] = altitude;
			data[4] = speed;
			data[5] = orientation;
			data[6] = pitch;
			data[7] = roll;
			data[8] = yawRate;
			data[9] = ax;
			data[10] = ay;
			data[11] = az;
			data[12] = slipAngle;
			data[13] = jerkX;
			data[14] = jerkY;
			data[15] = satelliteCount.valid ? satelliteCount.value : OptionalDouble();
			data[16] = antennaLongitude;
			data[17] = antennaLatitude;
			data[18] = antennaAltitude;
			data[19] = (Double)(Int)gnssTimeType;
			data[20] = arrivalTime;
			data[21] = gnssTime.isValid() ? gnssTime.refValue().dateTime.year : OptionalDouble();
			data[22] = gnssTime.isValid() ? gnssTime.refValue().dateTime.month : OptionalDouble();
			data[23] = gnssTime.isValid() ? gnssTime.refValue().dateTime.day : OptionalDouble();
			data[24] = gnssTime.isValid() ? gnssTime.refValue().dateTime.hour : OptionalDouble();
			data[25] = gnssTime.isValid() ? gnssTime.refValue().dateTime.minute : OptionalDouble();
			data[26] = gnssTime.isValid() ? gnssTime.refValue().dateTime.second : OptionalDouble();
			data[27] = gnssTime.isValid() ? (Double)gnssTime.refValue().milliseconds : OptionalDouble();

			sample.significantCount = 28;
			return sample;
		}

		Bool fromGeneralSample(GeneralSample sample)
		{
			return fromGeneralSample(getProtocolName(UINF), sample);
		}

		Bool fromGeneralSample(String protocol, GeneralSample sample)
		{
			if (protocol == "gnssimu-sample-v6")
			{
				if (sample.values.size() != 28) return FALSE;

				timeStamp = sample.timeStamp;

				locationMode = sample.values[0].valid ? (GnssImuLocationMode)(Int)sample.values[0].value : GILM_NoLocation;
				longitude = sample.values[1];
				latitude = sample.values[2];
				altitude = sample.values[3];
				speed = sample.values[4];
				orientation = sample.values[5];
				pitch = sample.values[6];
				roll = sample.values[7];
				yawRate = sample.values[8];
				ax = sample.values[9];
				ay = sample.values[10];
				az = sample.values[11];
				slipAngle = sample.values[12];
				jerkX = sample.values[13];
				jerkY = sample.values[14];
				satelliteCount = sample.values[15].valid ? (Int)sample.values[15].value : OptionalInt();
				antennaLongitude = sample.values[16];
				antennaLatitude = sample.values[17];
				antennaAltitude = sample.values[18];

				gnssTimeType = sample.values[19].valid ? (GnssTimeType)(Int)sample.values[19].value : GTT_UTC;
				arrivalTime = sample.values[20];
				if (sample.values[21].valid &&
					sample.values[22].valid &&
					sample.values[23].valid &&
					sample.values[24].valid &&
					sample.values[25].valid &&
					sample.values[26].valid &&
					sample.values[27].valid)
				{
					gnssTime = TimeWithMS((UInt)sample.values[21].value,
						(UInt)sample.values[22].value,
						(UInt)sample.values[23].value,
						(UInt)sample.values[24].value,
						(UInt)sample.values[25].value,
						(UInt)sample.values[26].value,
						(UInt)sample.values[27].value);
				}
				else
				{
					gnssTime = Optional<TimeWithMS>();
				}

				return TRUE;
			}
			else if (protocol == "gnssimu-sample-v5")
			{
				if (sample.values.size() != 19) return FALSE;

				timeStamp = sample.timeStamp;

				locationMode = sample.values[0].valid ? (GnssImuLocationMode)(Int)sample.values[0].value : GILM_NoLocation;
				longitude = sample.values[1];
				latitude = sample.values[2];
				antennaAltitude = sample.values[3];
				speed = sample.values[4];
				orientation = sample.values[5];
				pitch = sample.values[6];
				roll = sample.values[7];
				yawRate = sample.values[8];
				ax = sample.values[9];
				ay = sample.values[10];
				az = sample.values[11];

				gnssTimeType = GTT_UTC;
				arrivalTime = sample.timeStamp.offset;
				if (sample.values[12].valid &&
					sample.values[13].valid &&
					sample.values[14].valid &&
					sample.values[15].valid &&
					sample.values[16].valid &&
					sample.values[17].valid &&
					sample.values[18].valid)
				{
					gnssTime = TimeWithMS((UInt)sample.values[12].value,
						(UInt)sample.values[13].value,
						(UInt)sample.values[14].value,
						(UInt)sample.values[15].value,
						(UInt)sample.values[16].value,
						(UInt)sample.values[17].value,
						(UInt)sample.values[18].value);
				}
				else
				{
					gnssTime = Optional<TimeWithMS>();
				}

				return TRUE;
			}
			else if (protocol == "gnssimu-sample-v4")
			{
				if (sample.values.size() != 18) return FALSE;

				timeStamp = sample.timeStamp;

				longitude = sample.values[0];
				latitude = sample.values[1];
				antennaAltitude = sample.values[2];
				speed = sample.values[3];
				orientation = sample.values[4];
				pitch = sample.values[5];
				roll = sample.values[6];
				yawRate = sample.values[7];
				ax = sample.values[8];
				ay = sample.values[9];
				az = sample.values[10];

				locationMode = longitude.valid && latitude.valid ? GILM_Normal : GILM_NoLocation;

				gnssTimeType = GTT_UTC;
				arrivalTime = sample.timeStamp.offset;
				if (sample.values[11].valid &&
					sample.values[12].valid &&
					sample.values[13].valid &&
					sample.values[14].valid &&
					sample.values[15].valid &&
					sample.values[16].valid &&
					sample.values[17].valid)
				{
					gnssTime = TimeWithMS((UInt)sample.values[11].value,
						(UInt)sample.values[12].value,
						(UInt)sample.values[13].value,
						(UInt)sample.values[14].value,
						(UInt)sample.values[15].value,
						(UInt)sample.values[16].value,
						(UInt)sample.values[17].value);
				}
				else
				{
					gnssTime = Optional<TimeWithMS>();
				}

				return TRUE;
			}

			return FALSE;
		}

		static GnssImuSample interpolate(GnssImuSample& s1, Double w1, GnssImuSample& s2, Double w2, GlobalTimestamp time)
		{
			return GnssImuSample();
		}
	};
}

#endif
